import gym
import math
import numpy as np
import matplotlib.pyplot as plt
from gym import spaces
from copy import deepcopy
from pathlib import Path
from collections import deque
from scipy.integrate import odeint
from shapely import geometry as geo
from shapely.plotting import plot_polygon
from path_plan_env.env import MAP
from path_plan_env.lidar_sim import LidarModel


# 雷达设置
SCAN_RANGE = 5  # 扫描距离
SCAN_ANGLE = 360  # 扫描范围(单位deg)
SCAN_NUM = 90  # 扫描点个数
SCAN_CEN = 48  # 中心区域index开始位置(小于SCAN_NUM/2)


class Test(object):
    def __init__(self):
        self.obstacles = MAP.obstacles
        self.lidar = LidarModel(SCAN_RANGE, SCAN_ANGLE, SCAN_NUM)
        self.lidar.add_obstacles(MAP.obstacles)

        self.x = 0
        self.y = -5
        self.yaw = 0

    def get_obs(self):
        self.points, self.curr_scan_pos = self.lidar.scan(
            self.x, self.y, self.yaw, mode=1
        )

    def plot(self):
        fig, ax = plt.subplots()
        points = np.array(self.curr_scan_pos)
        ax.plot(points[:, 0], points[:, 1], ".r")
        ax.plot(self.x, self.y, "sb")
        for o in self.obstacles:
            if o.geom_type != 'LineString':
                plot_polygon(o, ax=ax, facecolor="w", edgecolor="k", add_points=False)
            else:
                ax.plot(o.xy[0],o.xy[1],'-k')
        plt.axis("equal")
        plt.grid()
        plt.show()


if __name__ == "__main__":
    test = Test()
    test.get_obs()
    test.plot()
